#include "RMRPIDIO.h"
#include "RMRUtil.h"
#include "RMRMain.h"

/*
 * PART 1
 * 
 * PIDEncoder class
 * 
 * Like a normal Encoder but has a PIDGet() method to be called by PIDController
 * 
 */


/*
 * Constructors
 */

// constructor, passed aSlot, aChannel, bSlot, bChannel, [reverseDirection]
PIDEncoder::PIDEncoder(UINT32 aSlot, UINT32 aChannel, UINT32 bSlot, UINT32 bChannel, bool reverseDirection, CounterBase::EncodingType = CounterBase::k4X) :
	Encoder(aSlot, aChannel, bSlot, bChannel, reverseDirection) // setup encoder
{
	m_reversed = reverseDirection; 
	SetDefaults();
}

// constructor, passed aChannel, bChannel, [reverseDirection]
PIDEncoder::PIDEncoder(UINT32 aChannel, UINT32 bChannel, bool reverseDirection, CounterBase::EncodingType = CounterBase::k4X) :
	Encoder(aChannel, bChannel, reverseDirection) // setup encoder
{
	m_reversed = reverseDirection;
	SetDefaults();
}


// Defaults for configuration options
// set whenever an object is created
void PIDEncoder::SetDefaults(void)
{
	m_scaling = 1.0;
	m_mode = kPosition;
	m_enabled = false;
}



/* 
 * Scaling
 */

// adjust scaling -- what we multiply the # of encoder clicks (from the Get() method) by to get revolutions
void PIDEncoder::SetScaling(float scaling)
{
	m_scaling = scaling;
}

float PIDEncoder::GetScaling(void)
{
	return m_scaling;
}

/*
 * Is the encoder reversed?
 */
bool PIDEncoder::IsReversed()
{
	return m_reversed;
}


/*
 * PIDGet - interface to PIDController
 */

// if we are in speed mode (user called SetSpeedMode) return the current wheel speed
// if we are in position mode (user caled SetPositionMode) return the current wheel position
double PIDEncoder::PIDGet()
{
	if (m_mode == kSpeed) return GetSpeed();
	if (m_mode == kPosition) return GetPosition();
	
	// this should never happen...
	printf("Error: unknown mode %d...\n", m_mode);
	return GetSpeed();
}


/*
 * Calculate wheel position and speed
 */

// returns position in # of wheel revolutions
float PIDEncoder::GetPosition(void)
{
	return Get() * m_scaling;
}

// returns wheel speed in # of wheel revolutions per second
float PIDEncoder::GetSpeed(void)
{	
	// get current position and time
	INT32 now_x = Get();
	float now_t = GetClock();
	
	// compute change in position and change in time
	INT32 delta_x = now_x - m_last_x;
	float delta_t = now_t - m_last_t;
	
//	printf("t: last = %f, now = %f\n", m_last_t, now_t);
//	printf("x: last = %d, now = %d\n", m_last_x, now_x);
	
	// set the last position and time to be now
	m_last_x = now_x;
	m_last_t = now_t;
	
	// instead of dividing by zero, return 0
	if (delta_t == 0) return 0;
	
	// return average speed
	return delta_x / delta_t * m_scaling;
	
}


/*
 * Mode
 */
// Tell PIDEncoder that PIDGet() should return wheel speed 
void PIDEncoder::SetSpeedMode()
{
	m_mode = kSpeed;
}

// Tell PIDEncoder that PIDGet() should return wheel position
void PIDEncoder::SetPositionMode()
{
	m_mode = kPosition;
}

// is the encoder in speed mode?
bool PIDEncoder::IsSpeedMode()
{
	if (m_mode == kSpeed) return true;
	
	return false;
}


/*
 * Status inquiry
 */

// is the encoder enabled?
bool PIDEncoder::IsEnabled()
{
	return m_enabled;
}


/*
 * Start, Reset, Stop
 */

// Start the encoder listening for clicks
void PIDEncoder::Start()
{
	Encoder::Start();
	m_last_x = Get();
	m_last_t = GetClock();
	m_enabled = true;
//	m_timer.Start();
}

// reset the count of the encoder to zero
void PIDEncoder::Reset()
{
	Encoder::Reset();
	m_last_x = Get();
	m_last_t = GetClock();
//	m_timer.Reset();
}

// stop counting clicks
void PIDEncoder::Stop()
{
	Encoder::Stop();
	m_enabled = false;
//	m_timer.Stop();
}





/*
 * PART 2
 * 
 * PIDAbsoluteEncoder class
 * 
 * listen to a free-turning, digital absolute encoder
 * returns angle of the encoder (in range -1 .. 1) in PIDGet()
 * 
 */

/*
 * Constructor, Initializer, Destructor
 */

PIDAbsoluteEncoder::PIDAbsoluteEncoder(UINT32 slot, UINT32 channel) :
	abs_encoder_count(slot, channel)
{
	Init();
}

PIDAbsoluteEncoder::PIDAbsoluteEncoder(UINT32 slot) :
	abs_encoder_count(slot)
{
	Init();
}

// Destructor needed to avoid weird compile error
PIDAbsoluteEncoder::~PIDAbsoluteEncoder()
{
	delete &abs_encoder_count;
}

// Set defaults for member variables
void PIDAbsoluteEncoder::Init()
{
	//initialize counter (for the absolute encoder)
	abs_encoder_count.SetSemiPeriodMode(true);
	abs_encoder_count.Start();
	
	m_offset = 0;
	
	/*
	 * If left > right : crossover = 1024
	 * Else			   : crossover = 0
	 */
	
	m_min = 0;
	m_max = 1024;
	m_backwards = false;
}


/*
 * Member variable getters and setters...
 */

void PIDAbsoluteEncoder::SetMin(int min)
{
	m_min = min;
}
int PIDAbsoluteEncoder::GetMin()
{
	return m_min;
}

void PIDAbsoluteEncoder::SetMax(int max)
{
	m_max = max;
}
int PIDAbsoluteEncoder::GetMax()
{
	return m_max;
}

void PIDAbsoluteEncoder::SetBackwards(bool backwards)
{
	m_backwards = backwards;
}
bool PIDAbsoluteEncoder::IsBackwards()
{
	return m_backwards;
}

void PIDAbsoluteEncoder::SetOffset(int offset)
{
	m_offset = offset;
}
int PIDAbsoluteEncoder::GetOffset()
{
	return m_offset;
}


//Calibrates the min, max, and direction parameters based on the limit switches
// UNTESTED!!! USE AT YOUR OWN RISK!!!
// this probably will break the watchdog, confuse the pid, etc...
void PIDAbsoluteEncoder::AutoCalibrate(DigitalInput &left_limit_switch, DigitalInput &right_limit_switch, Jaguar &turn_motor, Watchdog &watchdog)
{
	printf("Begining auto calibration\n");
	
	float old_expiration = watchdog.GetExpiration();
	watchdog.SetExpiration(MAX_TIME_FOR_AUTOCALIBRATE);
	
	bool look_for_left;
	
	printf("driving to the left to find min angle\n");
	//STEER_CALIBRATE_SPEED
	turn_motor.Set(-0.16);
	while(true)
	{
		if (!left_limit_switch.Get())
		{
			m_max = (int) GetPulseWidth();
			m_backwards = false;
			printf("Left limit switch pressed at %d, normal direction\n", m_min);
			look_for_left = false;
			break;
		}
		if (!right_limit_switch.Get())
		{
			m_min = (int) GetPulseWidth();
			m_backwards = true;
			printf("Right limit switch pressed at %d, REVERSED direction\n", m_min);
			look_for_left = true;
			break;
		}
		watchdog.Feed();
	}
	
	printf("driving to the right to find max angle\n");
	
	//STEER_CALIBRATE_SPEED
	turn_motor.Set(0.20);
	Wait(0.5);

	turn_motor.Set(0.20);
	while(true)
	{
		if (look_for_left && !left_limit_switch.Get())
		{
			m_max = (int) GetPulseWidth();
			m_backwards = true;
			printf("Left limit switch pressed at %d, REVERSED direction\n", m_max);
			break;
		}
		if (!look_for_left && !right_limit_switch.Get())
		{
			m_min = (int) GetPulseWidth();
			m_backwards = false;
			printf("Right limit switch pressed at %d, normal direction\n", m_max);
			break;
		}
		watchdog.Feed();
	}
	
	//turn_motor.Set(0);
	
	if(m_min < m_max)
		m_offset = 0;
	else
		m_offset = (m_max + m_min) / 2;
	
	printf("Done with calibration! Min=%d, Max=%d, offset = %d, %s\n", m_min, m_max, m_offset, m_backwards ? "backwards" : "normal");
	watchdog.SetExpiration(old_expiration);
	
}



//Return pulse width of the absolute encoder signal
float PIDAbsoluteEncoder::GetPulseWidth(void)
{
	float pulse_width = (float) 1.0e6*abs_encoder_count.GetPeriod();
	return pulse_width;
}

// The PIDSource::PIDGet method
// return the angle, in terms of -1 is left and +1 is right
double PIDAbsoluteEncoder::PIDGet()
{
	static int i = 0;
	int period = DEBUG_OUT_PERIOD_PID;
	
	float pulse_width = GetPulseWidth();
	
	//printf("im here in absolute encoder PID get\n");
	UINT32 position; 
	
	if (m_backwards)
	{
		pulse_width = 1024 - pulse_width;
	}
	
	
	/*if (pulse_width < m_crossover)
	{
		position = ((INT32) pulse_width + m_crossover) % 1024;
	}
	else
	{
		position = (INT32) pulse_width;
	}*/
	position = ((INT32) pulse_width + m_offset) %1024;
	
	//just min and max by offset
	int min_with_offset = (m_min - m_offset) % 1024;
	int max_with_offset = (m_max - m_offset) % 1024;
	
	if (max_with_offset < 0)
		max_with_offset = max_with_offset + 1024;

	float angle = ScaleRangeToRange(position, min_with_offset, max_with_offset, -1.0, 1.0);
	
	
	
	// Debug printout stuff
	i++;
	if (i > period)
	{
		i = 0;
		//printf("min_with_offset:%f max_with_offset:%f\n", min_with_offset, max_with_offset);
		//printf("pulse width %f with crossover at %d is mapped to %f (in range -1 .. 1)\n", pulse_width, m_crossover, angle);
	//	printf("Pulse Width of %f turned into position of %d and angle of %f\n", pulse_width, position, angle);

	}
	
	
	return angle;
}





/*
 * PART 3
 * 
 * PIDJaguar class
 * 
 * Like a normal Jaguar but has a PIDWrite() method to be called by PIDController
 * 
 */


/*
 * Constructors
 */

// constructor, passed channel
PIDJaguar::PIDJaguar(UINT32 channel) :
	Jaguar(channel) // setup Jaguar
{
	SetDefaults();
}

// constructor, passed slot and channel
PIDJaguar::PIDJaguar(UINT32 slot, UINT32 channel) :
	Jaguar(slot, channel)
{
	SetDefaults();
}

// Defaults for configuration options
// set whenever an object is created
void PIDJaguar::SetDefaults(void)
{
	m_max_wheel_acceleration = MAX_WHEEL_ACCEL;
	m_direction = 1;
	m_last_speed = 0.0;
	m_limit_acceleration_enabled = true;
}

// switch on and off the acceleration limiting
void PIDJaguar::EnableLimitAcceleration(void)
{
	m_limit_acceleration_enabled = true;
}

void PIDJaguar::DisableLimitAcceleration(void)
{
	m_limit_acceleration_enabled = false;
}




/*
 * PIDWrite - interface to PIDController
 */

// Set the jaguar to the specefied output (in range -1 .. 1)
void PIDJaguar::PIDWrite(float output)
{
	static int i = 0;
	int period = DEBUG_OUT_PERIOD;
	
	float target_speed;
	
	if (m_limit_acceleration_enabled)
		target_speed = LimitAcceleration( m_direction * Limit(output) );
	else
		target_speed = m_direction * Limit(output);
	
	i++;
	if (i > period)
	{
		i = 0;
		//printf("PIDJaguar set to %f\n", target_speed);
	}
	
	
	Set(Limit(target_speed));
	//printf("PIDJaguar was writed too\n");
}

// send speed in, get acceleration-limited speed out
float PIDJaguar::LimitAcceleration(float speed)
{
	static int i = 0;
	int period = DEBUG_OUT_PERIOD_PID;
	
	// compute change in speed since last time this function was called
	float delta_speed = speed - m_last_speed;
	
	// debug stuff... print these lines once every period loops
	i++;
	if (i > period) {
		i = 0;
		//printf("Speed,Delta,Last = %8.4f, %8.4f, %8.4f", speed, delta_speed, m_last_speed);
	}
	
	//printf("Hi from LimitAcceleration!\n");
	
	if (delta_speed > m_max_wheel_acceleration)
	{
		delta_speed = m_max_wheel_acceleration;
	}
	if (delta_speed < -m_max_wheel_acceleration)
	{
		delta_speed = -m_max_wheel_acceleration;
	}
	speed = m_last_speed + delta_speed;
	
	if (i == 0)
	{
		//printf(" NOW = %8.4f, %8.4f, %8.4f\n", speed, delta_speed, m_last_speed);
	}
	
	m_last_speed = speed;
	
	return speed;
}



/*
 * Switch between forward and backward
 */

// if inverted is true, the motor will be driven backwards
void PIDJaguar::SetInverted(bool inverted)
{
	m_direction = inverted ? -1 : 1;
}

// returns true if the motor direction is inverted
bool PIDJaguar::GetInverted()
{
	return m_direction == -1;
	
}

// returns -1 if inverted, +1 otherwise
INT32 PIDJaguar::GetDirection()
{
	return m_direction;
}





/*
 * PART 4
 * 
 * PIDOutputDuplicator class
 * 
 * Like a normal PIDOutput but sets two PIDOuput subobjects in the PIDWrite method
 * 
 */

PIDOutputDuplicator::PIDOutputDuplicator(PIDOutput *outputA, PIDOutput *outputB) :
	m_outputA(outputA),
	m_outputB(outputB)
{
	
}

void PIDOutputDuplicator::PIDWrite(float output)
{
	m_outputA->PIDWrite(output);
	m_outputB->PIDWrite(output);
}

